import sys,copy,dwrap,numpy as np
#sys.path.append("/home/pr200/libdai/swig/")
#sys.path.append("./swig")
sys.path.append("/home/alfredo/Desktop/libDAI-0.3.1/swig")  #"/usr/bin/swig-1.3"
#sys.path.append("/Users/trananh/Workspace/CC++/libdai/swig")
#sys.path.append("/Users/tpilanta/Documents/workspace/libdai/swig")
#sys.path.append("/home/tokutomi/Downloads/libdai/swig")
import dai
from utils import NodeNames as NN
from rjwrap import pchangepoints as pcp
from decimal import Decimal, ROUND_DOWN;

##to correct
##getting -1s in output
##check if bridging occurs with location


class STCm:
    ## stc manager class
    def __init__(self, STC, actors):
        ##also want time segmented into indexed time intervals.
        tracks = [x.track for x in actors]
        self.actors = actors
        if len(tracks) > 0:
            ttuple = self.buildtimelines(STC,tracks)
            self.timelines = ttuple[0]
            self.grouplist = ttuple[1]
            self.indexlist = ttuple[2]
            self.gidlist = ttuple[3]
            self.movlen = ttuple[4]
            self.gtime = ttuple[5] ##global discretized time
            self.ptime2gtime = ttuple[6] ##personal timeline to global time (above)
            self.glist = ttuple[7]
            self.glist2gtime = ttuple[8]
            self.glist2ptime = ttuple[9]
            self.ptime2glist = ttuple[10]
            self.pchangepoints = self.buildpchangepoints()
            self.buildolist(self.timelines)
            self.buildilookup(self.timelines)
            self.movlen = len(STC)

    def writeout(self):
        print "actors\n---\n"
        print self.actors
        print "timelines\n---\n"
        print self.timelines
        print "grouplist\n---\n"
        print self.grouplist
        print "indexlist\n---\n"
        print self.indexlist
        print "gidlist\n---\n"
        print self.gidlist
        print "movlen\n---\n"
        print self.movlen
        print "gtime\n---\n"
        print self.gtime
        print "ptime2gtime\n---\n"
        print self.ptime2gtime
        print "glist\n---\n"
        print self.glist
        print "glist2gtime\n---\n"
        print self.glist2gtime
        print "glist2ptime\n---\n"
        print self.glist2ptime
        print "ptime2glist\n---\n"
        print self.ptime2glist
        print "pchangepoints\n---\n"
        print self.pchangepoints
        print "olist\n---\n"
        print self.olist
        print "ilookup\n---\n"
        print self.ilookup
    

    ## sets timelines, grouplist, indexlist, gidlist, movlen
    ## timelines have format [ginfo1, ginfo2, ...]
    ## with length equal to the number of unique groupings
    ##   where ginfo has format ([gmembers], (stime,etime))
    ## gidlist has format [groupstring1, groupstring2, ...]
    ##   where groupstring has format "m1,m2,...-stime"
    ## indexlist has format [is1, is2, ...]
    ##   where is has format [gid1, gid2, ...]
    ## grouplist has format [group1, group2, ...]
    ##   where group has format [m1, m2, ...]
    def buildtimelines(self,STC,tracks):
        na = len(tracks)
        timelines = [[] for x in range(na)]
        prevgroups = range(na)
        grouplist = []
        indexlist = []
        gidlist = []
        glist = []
        movlen = len(STC)
        prevgroupings = map(lambda x: [x],range(na))
        gtime = []
        stime = 0
        ptime2gtime = [[] for x in range(na)]
        framedtracks = []
        for track in tracks:
            ttrack = [(i,x) for i,x in enumerate(track)]
            framedtracks.append(ttrack)
        aframes = []
        for track in framedtracks:
            tlist = []
            for frame in track:
                if frame[1][2] > .5:
                    tlist = []
                    tlist.append(frame)
                else:
                    tlist.append(frame)
            aframes.append(tlist)
        #print aframes
        naframes = [(x[1] if (len(x) > 1) else [movlen+1]) for x in aframes]    
        
        for i,line in enumerate(STC):
            gtimeupdated = False
            #print "line ",line
            indices = line
            indexlist.append(indices)
            cgroups = [[] for x in range(na)]
            #print indices
            for person,index in enumerate(indices):
                #print person," ",index
                cgroup = copy.deepcopy(cgroups[index])
                cgroup.append(person)
                cgroups[index] = cgroup
            grouplist.append(cgroups)
            #print cgroups
            ioutarr = [tracks[x][i][2] < 1.0 for x in range(na)]
            woutarr = [tracks[x][i-1][2] < 1.0 for x in range(na)] if i > 0 else [True]*na
            for group in cgroups:
                #print group
                for person in group:
                    #isout = tracks[person][i][2] < 1.0
                    isout = ioutarr[person]
                    #wasout = tracks[person][max(i-1,0)][2] < 1.0
                    wasout = woutarr[person]
                    #if person == 3:
                    #    print group
                    #    print prevgroupings[person]
                    #print "person ",person
                    #print "prevgroupings ",prevgroupings
                    #print "prevgroups ",prevgroups
                    if (prevgroupings[prevgroups[person]] != group) or (isout != wasout):
                        #print "???"
                        if not isout:
                            if (len(group)>0):
                                # print [group,i]
                                # print glist
                                # print [group,i] in glist
                                if not [group,i] in glist:
                                    #print "[group,i] ",[group,i]
                                    glist.append([group, i])
                                # print glist
                                # print "\n\n"
                            if len(group) > 1:
                                gmems = ",".join(map(str,group))
                                gid = gmems+"-"+str(i)
                                if not gid in gidlist:
                                    gidlist.append(gid)
                                # else:
                                #     print gidlist
                                #     print "skipped ",gid
                                #     print "\n\n"
                            elif len(group) > 0:
                                gmems = str(person)
                                gid = gmems+"-"+str(i)
                                if not gid in glist:
                                    gidlist.append(gid)
                                # else:
                                #     print gidlist
                                #     print "skipped ",gid
                                #     print "\n\n"
            if sum(woutarr) == na:
                stime = i
            for person,index in enumerate(indices):
                #isout = tracks[person][i][2] < 1.0
                isout = ioutarr[person]
                #wasout = tracks[person][max(i-1,0)][2] < 1.0
                wasout = woutarr[person]
                if (isout != wasout) or (prevgroupings[prevgroups[person]] != cgroups[index]):
                    #print "i ",i," person ",person," isout ",isout," wasout ",wasout
                    if not gtimeupdated:
                        if (i > 0) and (sum(woutarr) < na):
                            #print "i ",i
                            #print "i ",i," movlen ",movlen
                            #print "???stime ",stime," i ",i
                            gtime.append((stime, i-1))
                            #print "!!!stime ",stime," i ",i
                        stime = i
                        #print "stime ",stime
                        gtimeupdated = True
                    if not isout:
                        cptime2gtime = copy.deepcopy(ptime2gtime[person])
                        cptime2gtime.append([])
                        ptime2gtime[person]=cptime2gtime
                    ctimeline = copy.deepcopy(timelines[person])
                    if len(ctimeline) > 0:
                        lastint = ctimeline[len(ctimeline)-1]
                        tend = lastint[1]
                        app = False
                        while True:
                            try:
                                len(tend)
                                tend = tend[-1]
                                app = True
                            except TypeError:
                                break
                        #print "---\n\n",lastint[0]," ",tend
                        newint = (lastint[0],(tend,i-1))
                        if app:
                            ctimeline.append(newint)
                        else:
                            ctimeline[len(ctimeline)-1] = newint
                    if not isout:
                        ctimeline.append((cgroups[indices[person]],i))
                    timelines[person] = ctimeline
                    
            if gtimeupdated:                
                for person,index in enumerate(indices):
                    isout = tracks[person][i][2] < 1.0
                    if not isout:
                        #print "person ",person," len(gtime) ",len(gtime)
                        if len(ptime2gtime[person]) > 0:
                            cptime2gtime = copy.deepcopy(ptime2gtime[person])
                            tcptime2gtime = copy.deepcopy(cptime2gtime[-1])
                            tcptime2gtime.append(len(gtime))
                            cptime2gtime[-1] = tcptime2gtime
                        else:
                            cptime2gtime = [[len(gtime)]]
                        ptime2gtime[person]=cptime2gtime
                        #gtime2ptime[person].append({len(gtime):len(ptime2gtime[person]-1)})
                            
            prevgroups = indices
            prevgroupings = cgroups
        gtimeupdated = False
        sframes = []
        for person in range(na):
            #print timelines[person]
            if len(timelines[person]) > 0:
                lastint = timelines[person][-1]
                try:
                    lastint[1][1]
                except TypeError:
                    gtimeupdated = True
                    #print "movlen ",movlen
                    sframes.append(lastint[1])
                    timelines[person][-1] = (lastint[0],(lastint[1],movlen-1))
        if gtimeupdated:
            gtime.append((max(sframes),movlen-1))
        
        for index,naframe in enumerate(naframes):
            frame = naframe[0]
            if len(timelines[index]) > 0:
                tline = copy.deepcopy(timelines[index])
                if len(tline) > 0:
                    lgroup = tline[-1][0]
                    lint = tline[-1][1]
                    try:
                        len(lint)
                    except TypeError:
                        tline[-1] = (lgroup,(lint,naframes[index][0]-1))
                timelines[index] = tline
        if gtimeupdated:
            indices = STC[-1]
            #print "INDICES! ",indices
            for person,index in enumerate(indices):
                isout = tracks[person][movlen-1][2] < 1.0
                #print "ISOUT!!!!!!!!!!!!!!!!!!!! ",isout
                if not isout:
                    #print "person ",person," len(gtime) ",len(gtime)
                    if len(ptime2gtime[person]) > 0:
                        cptime2gtime = copy.deepcopy(ptime2gtime[person])
                        tcptime2gtime = copy.deepcopy(cptime2gtime[-1])
                        tcptime2gtime.append(len(gtime))
                        cptime2gtime[-1] = tcptime2gtime
                    else:
                        cptime2gtime = [[len(gtime)]]
                        ptime2gtime[person]=cptime2gtime
        glist2gtime = []
        glist2ptime = []

        #print "glist\n",glist
        #print "gtime\n",gtime
        for k,g in enumerate(glist):
            stime = g[1]
            #print "g ",g
            #print "stime ",stime," group ",g[0]," gtime ",gtime
            for i,time in enumerate(gtime):
                if time[0] == stime:
                    #print "found"
                    start = i
                    cglist2ptime = dict()
                    for idd in g[0]:
                        etime = movlen-1
                        #print "timelines[idd] ",timelines[idd]
                        for t,tup in enumerate(timelines[idd]):
                            #print tup
                            if tup[1][0] == stime:
                                cglist2ptime[idd]=t
                                etime = tup[1][1]
                                #print "stime ",stime," etime ",etime
                                for j,timeint in enumerate(gtime):
                                    if timeint[1] == etime:
                                        end = j
                                try:
                                    temp = end
                                except UnboundLocalError:
                                    end = len(gtime)
                                        #print start, end
                                        
                            
                    #print stime, etime
                    glist2gtime.append(range(start, end+1))
                    glist2ptime.append(cglist2ptime)
                    break
            #print "stime ",stime
            #print "g ",g
            #print timelines
            cg = copy.deepcopy(g)
            cg[1] = [stime, etime]
            glist[k] = cg
            #print "k ",k," glist[k] ",glist[k]
        
        #print "glist\n",glist
        #print "timelines\n",timelines
        ptime2glist = []
        def tequalsg(t, g):
            peoplet = t[0]
            peopleg = g[0]
            if not (len(peoplet) == len(peopleg)):
                return(False)
            for i,person in enumerate(peoplet):
                if not (person == peopleg[i]):
                    return(False)
            timet = t[1]
            timeg = g[1]
            if not (timet[0] == timeg[0] and timet[1] == timeg[1]):
                return(False)
            return(True)
        for (actor,timeline) in enumerate(timelines):
            #print "actor ",actor
            #print "timeline\n",timeline
            #print "glist\n",glist
            thisptime2glist = dict()
            for i,t in enumerate(timeline):
                for j,g in enumerate(glist):
                    #print "i ",i," t ",t," j ",j," g ",g
                    ##compare
                    #print g
                    if tequalsg(t,g):
                        #print "!!!"
                        thisptime2glist[i]=j
                        break
            ptime2glist.append(thisptime2glist)

        return (timelines,grouplist,indexlist,gidlist,movlen,gtime,ptime2gtime,glist,glist2gtime,glist2ptime,ptime2glist)
    def buildpchangepoints(self):
        pchangepoints = []
        #print "!!!\n\n\n",self.timelines
        for actor,actorstimeline in enumerate(self.timelines):
            pchangepoints_actor = []
            for t in actorstimeline:
                #print "t ",t
                start = t[1][0]
                end = t[1][1]
                #print "start ",start
                #print "end ",end
                d1 = []
                d2 = []
                t = []
                for f in range(start,end):
                    d1.append(self.actors[actor].track[f][0])
                    d2.append(self.actors[actor].track[f][1])
                    t.append(f)
                cps = pcp(t,d1,d2,1.0/30.0,10000,all=False)
                pchangepoints_actor.append(cps)
            pchangepoints.append(pchangepoints_actor)
        return pchangepoints
                    
            
            
    ## builds lists for each actor with same format as timeline, except
    ## list of actors doesn't contain the actor in question
    ## i.e. for person 1, [0,1,2] becomes [0,2]
    def buildolist(self,timelines):
        olist = []
        for actor,timeline in enumerate(timelines):
            oline = []
            for entry in timeline:
                actors = entry[0]
                times = entry[1]
                others = filter(lambda x: x!=actor,actors)
                oline.append((others,times))
            olist.append(oline)
        self.olist = olist

    ## builds dictionary keyed by groupid and then keyed again by actor
    ## each entry contains the index in that actor's timeline that this grouping
    ## corresponds to
    def buildilookup(self,timelines):
        ilookup = dict()
        for actor,timeline in enumerate(timelines):
            for ind,entry in enumerate(timeline):
                actors = entry[0]
                time = entry[1][0]
                gkey = self.mkey(actors,time)
                if not gkey in ilookup.keys():
                    ilookup[gkey] = dict()
                if not actor in ilookup[gkey].keys():
                    ilookup[gkey][actor] = dict()
                ilookup[gkey][actor] = ind
        self.ilookup = ilookup

    def mkey(self,actors,time):
        return "-".join(((str(actors[0]) if (len(actors) == 1) else ",".join(map(str,actors))),str(time)))
    def gid2tuple(self,gid):
        gid = gid.split("-")
        return (gid[0].split(","),gid[1])
    def inside(self,gr1, gr2):
        for i in gr1:
            if not i in gr2:
                return(False)
        return(True)
    def setminus(self,gr2, gr1):
        gr3 = copy.deepcopy(gr2)
        for i in gr1:
            gr3.remove(i)
        return gr3
        

class Actor:
    i = 0
    def __init__(self, track=None, gestures = [], looks = [], crouches = [], kinetics = [], military = None, edges = [], ignore=False):
        self.track = track
        self.gestures = gestures
        self.looks = looks
        self.crouches = crouches
        self.edges = edges
        self.kinetics = kinetics
        self.military = military
        self.edges = edges
        self.idd = Actor.i
        self.ignore = ignore
        Actor.i += 1

class Node:
    def __init__(self, name="", sId=0, actName="", aSubjects=[], aActorsInfo=[], 
                 location="", timeId=0, aTimes=[], nRealInitT=0, aScores=[], sParentsId=[], s = [], o = [], t = [], l = [], 
                 shared = True, extendable=True, weight = 1):
        self.name = name
        self.sId = sId;
        self.actName = actName;
        self.aSubjects = aSubjects;
        self.aActorsAll = aActorsInfo[0] if len(aActorsInfo)>0 else [];
        self.aActorTypes = aActorsInfo[1] if len(aActorsInfo)>1 else [];
        self.aActorTimes = aActorsInfo[2] if len(aActorsInfo)>2 else [];
        self.aActorRoles = aActorsInfo[3] if len(aActorsInfo)>3 else [];
#         self.aSubjectTypes = aSubjectTypes;
#         self.aObjects = aOtherArgs[0] if len(aOtherArgs)>0 else [];
#         self.aObjectTimes = aOtherArgs[1] if len(aOtherArgs)>1 else [];
#         self.aObjectTypes = aOtherArgs[2] if len(aOtherArgs)>2 else [];
#         self.aInd_Objects = aOtherArgs[3] if len(aOtherArgs)>3 else [];
#         self.aInd_ObjectTimes = aOtherArgs[4] if len(aOtherArgs)>4 else [];
#         self.aInd_ObjectTypes = aOtherArgs[5] if len(aOtherArgs)>5 else [];
        self.location = location;
        self.timeId = timeId; #REMOVE Later (used only for making Rnd Graphs [but can be used as id="endT.sId"])
        self.aTimes = aTimes;
        self.nRealInitT = nRealInitT;
        self.aScores = aScores;
        self.sParentsId = sParentsId;
        self.mForward = dict(); #Forward = Likelihood * max[Trans * PastFwd]. (one for each HMM of each subject that appears in this node)
        self.mMaxPast_NodeId = dict(); #NodeID to Max Past (Parent) Node (one for each HMM of each subject that appears in this node)
        self.mHmmNullProb = dict();
#        self.mNormalizers = dict();
        self.s = s
        self.o = o
        self.t = t
        self.l = l
        self.shared = shared
        self.extendable=extendable
        self.weight = weight
        self.attributes = dict()
        
#    def getHmmProb(self, sArgSubject_Hmm):
#        return self.nHmmProb/self.mNormalizers[sArgSubject_Hmm];
#        
#    def getHmmNullProb(self, sArgSubject_Hmm):
#        return self.nHmmNullProb/self.mNormalizers[sArgSubject_Hmm];
        
    def set_attribute(self, k, v):
        self.attributes[k] = v
    
    def get_attribute(self, k):
        return self.attributes[k]

class Graph:
    def __init__(self):
        self.ndict = dict()
        self.idndict = dict();
        self.obs = []
        self.indirect_parents = dict()
        self.private_parents = dict()
        self.setupped = False
        self.mexnodes = []
        self.ran= False
    
    def which(self,name = None, s = None, o = None, l = None, t = None):
        li = []        
        names = name
        ss = s
        os = o
        ls = l
        ts = t
        for node in self.ndict.keys():
            if name == None:
                name = node.name
            if s == None:
                s = copy.deepcopy(node.s)
            if o == None:
                o = copy.deepcopy(node.o)
            if t == None:
                t = copy.deepcopy(node.t)
            if l == None:
                l = copy.deepcopy(node.l)
            if name == node.name and s == node.s and o == node.o and l == node.l and t == node.t:
                li.append(node)
            name = names
            s = ss
            o = os
            l = ls
            t = ts
        return li
            
    def addnode(self,node,parents,cpt):
        self.idndict[node.sId] = node;
        self.ndict[node] = [parents, cpt]
        return(0)
            
    def removeNode(self, node):
        if self.idndict.has_key(node.sId):
            del self.idndict[node.sId];
        if self.ndict.has_key(node):
            del self.ndict[node];
        return(0);
 
    def addobs(self, node, value=False, prior=0.5):
        for i in range(len(self.obs)):
            if self.obs[i][0] == node:
                self.obs[i] = [node, value, prior];
                return(0);
        self.obs.append([node, value, prior]);
        return(0)
    
    def removeObs(self, node): #, value=False, prior=0.5):
        for i in range(len(self.obs)):
            if self.obs[i][0] == node:
                self.obs.remove(self.obs[i]);
                return(0);

    #use sparingly! Will prob need to make this faster if there are loads of potential mutually exclusive nodes
    
    def mex(self,nodes):
        cpt = [0]*(1 << len(nodes))
        cpt[0] = 1.0
        for i in range(0, len(nodes)):
            cpt[1 << i] = 1.0
        y = Node("Mex", shared=False)
        self.addnode(y, nodes, cpt)
        self.mexnodes.append(y)

    def exactlyonetrue(self,nodes):
        cpt = [0]*(1 << len(nodes))
        for i in range(0, len(nodes)):
            cpt[1 << i] = 1.0
        y = Node("Mex", shared=False)
        self.addnode(y, nodes, cpt)
        self.mexnodes.append(y)


    ##These are suboptimal, pls don't use with too many nodes
    def any(self,node,parents):
        cpt = [1]*(1 << len(parents))
        cpt[0] = 0
        self.addnode(node, parents, cpt)
        
    def all(self,node,parents):                      
        cpt = [0]*(1 << len(parents))
        #print(cpt)
        cpt[len(cpt)-1] = 1        
        self.addnode(node, parents, cpt)
    
    def sample(self, parents, probs=[]):
        ##do not call with less than 2 parents
        private_parents = []
        if len(probs)==0:
            probs = [1./len(parents)]*len(parents)
        y = Node("Internal", shared=False)
        yold = y
        private_parents.append(y)
        sp = sum([probs[0], probs[1]])
        spold = sp        
        if sp > 0.00000001:
            stw = map(lambda x: float(x)/spold, [probs[0],probs[1]])
        else:
            #doesn't matter - they'll be ignored
            stw = [0.5,0.5]
        self.addnode(y, [parents[0], parents[1]], [0.0, stw[1], stw[0], 1.0])
        # if self.name(parents[0]) == "approach-join_2_0_34":
        #     print self.name(parents[0]), self.name(parents[1])
        #     print [0.0, stw[1], stw[0], 1.0]
        #     print map(lambda x: x.weight == "f", [parents[0], parents[1]])

        if len(parents) > 2:
            for i in range(2, len(parents)):
                y = Node("Internal",shared=False)
                private_parents.append(y)
                sp = spold + probs[i]
                if sp > 0.00000001:
                    stw = map(lambda x: float(x)/sp, [spold,probs[i]])
                else:
                    stw = [0.5,0.5]
                self.addnode(y, [yold, parents[i]], [0.0, stw[1], stw[0], 1.0])
                yold = y
                spold = sp
        self.indirect_parents[y]=parents
        private_parents.remove(y)
        self.private_parents[y]=private_parents
        return y
            
            
        

    # def sample(self,node,parents,probs=[]):
    #     if len(probs)==0:
    #         probs = [1./len(parents)]*len(parents)
    #     #print len(parents)
    #     cpt = []
    #     for i in range(1 << len(parents)):            
    #         digits = self.tobin(i, len(parents))        
    #         count = 0
    #         for i,digit in enumerate(digits):                
    #             if digit:
    #                 count += probs[i]
    #         cpt.append(float(count))
    #     self.addnode(node, parents, cpt)
    
    def setup(self):
        #nndict stands for "named node dictionary"
        if (self.setupped):
            return(0)
        nndict = dict()
        for node in self.ndict.keys():
            nndict[self.name(node)] = ["latent", self.name(self.ndict[node][0]), self.ndict[node][1]]
        def toposort(nodedict):
            visited = dict()
            nodes = nodedict.keys()
            nodelist = []

            def visit(node):
#                print node.name;
                try:
                    visited[node]
#                    print "visited!";
                except KeyError:
                    visited[node] = 1                    
                    pset = nodedict[node][0] ## parents                    
                    for p in pset:
                        visit(p)
#                    print "Appending:", node.name;
                    nodelist.append(node)
            for node in nodedict.keys():
#                 print "Starting with:", node.name;
                visit(node)
#             print "";
#             for node in nodelist:
#                 print "Ending with:", node.name;
            return(nodelist)
        def p2cpt(pset, parents, ptable, node2label):
            def getIndex(aVals, aBase, aIndex):
                nIndex = 0;
                for i, nV in enumerate(aVals):
                    nF, nBases = 1, len(aBase[i+1:]);
                    for j in range(nBases):
                        nF *= aBase[aIndex[j+1]];
                    nIndex += (nV*nF);
                return nIndex;            
            def getPermutation(nIndex, aBase):
                aVals = [];
                for nB in aBase[::-1]:
                    aVals.append(nIndex % nB);
                    nIndex /= nB;
                aVals.reverse();
                return aVals;
            def permute(self,ptable, ids):
                order = [i[0] for i in sorted(enumerate(ids), key=lambda x:x[1])]
                pnew = [0]*len(ptable)
                for (i,val) in enumerate(ptable):    
                    d = self.tobin(i, len(ids))
                    od = [d[j] for j in order]
                    newi = self.frombin(od)
                    if newi>len(pnew)-1:
#                        newi = len(pnew)-1;
                        print "ERROR (newi>len(pnew)-1): Node's CPT has incorrect number of probability arguments.";
                    pnew[newi] = val
                return pnew
            
            aV = [2,2,2];
            nVals = 1;
            for n in aV:
                nVals *= n;
            aFrom, aTo = [0,1,2], [2,0,1];
            aDiff = [aTo.index(i) for i in range(len(aTo))]; #[1,2,0]
            aV2 = [aV[i] for i in aTo]; #[2,3,2]

            ids, aTo = [], [];
            for parent in parents:
                ids.append(node2label[parent])
                aTo.append(node2label[parent])
            ptable = permute(self, ptable,ids)
#             print "aTo:", aTo;

            plen = len(ptable)
#             tset = [0]*2*plen
            tlen = np.log2(plen)+1 ## length our binary string
            for i,entry in enumerate(ptable):
                entry = 1-entry
                ## entries all assume entry conditioned on others is 0
                ## ptables count up across the remaining variables in binary
                ## so e.g. for 3 variables (c|a,b), the table looks like
                ## 0 0 0
                ## 0 0 1
                ## 0 1 0
                ## 0 1 1
                ## however, dai requires that the full table be specified, and
                ## counts in "reversed" binary, so the full table for the above
                ## example would be ordered:
                ## 0 0 0
                ## 1 0 0
                ## 0 1 0
                ## 1 1 0
                ## 0 0 1
                ## 1 0 1
                ## 0 1 1
                ## 1 1 1
                tstring = bin(i) ## this string has format '0bxxxxx'
                # print("tstring",tstring)
                tstring = tstring[2:] ## strip off leading "0b"
                tstring = "0"*(tlen-len(tstring))+tstring
                # print("tstring",tstring)
                tstring = tstring[1:] ## don't want to reverse first digit
                rstring = tstring[::-1]
                ## reverse the slice, this gives us the binary number we want
                s1 = "0"+rstring
                # print("s1",s1)
                ind = int(s1,2) ## where in the list we want to put this prob
                # print("ind",ind)
                # print("entry",entry)
                # print("1-entry",1-entry)
                # print("ind+plen",ind+plen)
                pset[ind] = entry
                pset[ind+plen] = 1-entry
            
#             for i in range(len(ptable)*2):
#                 print "["+str(i)+"]", pset[i]; 
            return pset

        def buildbridges():
            name2nodes = dict()
            for node in self.ndict.keys():
                if (node.shared):
                    nname = self.name(node)
                    if nname in name2nodes.keys():
                        name2nodes[nname].append(node)
                    else:
                        name2nodes[nname] = [node]
            name2bridge = dict()
            bridges = []
            for nname in name2nodes.keys():
                if len(name2nodes[nname])>1:                  
                    tobridge = name2nodes[nname]
                    probs = map(lambda x: x.weight, tobridge)
                    if ("f" in probs):
                        for i,p in enumerate(probs):
                            if p=="f":
                                probs[i]=1.
                            else:
                                probs[i]=0.
                    s = sum(probs)
                    probs = map(lambda x: float(x)/s, probs)
                    # if nname == "approach-join_2_0_34":
                    #     print self.name(name2nodes[nname])
                    #     print probs
                    #     print map(lambda x: x.weight == "f", name2nodes[nname])
                    bridge = self.sample(tobridge, probs)
                    bridge.name = "bridge_to_"+nname
                    nodes = name2nodes[nname]
                    assert( len(set([str(sorted(n.s)) for n in nodes])) == 1 )
                    assert( len(set([str(sorted(n.o)) for n in nodes])) == 1 )
                    assert( len(set([str(sorted(n.t)) for n in nodes])) == 1 )
                    bridge.s = copy.deepcopy(nodes[0].s)
                    bridge.o = copy.deepcopy(nodes[0].o)
                    bridge.l = copy.deepcopy(nodes[0].l)
                    bridge.t = copy.deepcopy(nodes[0].t)
                    bridge.shared = False
                    bridges.append(bridge)
                    name2bridge[nname] = bridge

            for node in self.ndict.keys():                
                if not node in bridges and not sum(map(lambda x: node in x, self.private_parents.values())):
                    
                    for i,parent in enumerate(self.ndict[node][0]):
                        if parent.shared and self.name(parent) in name2bridge:
                            self.ndict[node][0][i] = name2bridge[self.name(parent)]
            def identical(n1, n2):
                #will only call if already know they have the same name
                prts1 = self.ndict[n1][0]
                cpt1 = self.ndict[n1][1]
                prts2 = self.ndict[n2][0]
                cpt2 = self.ndict[n2][1]
                if len(prts1) == len(prts2):
                    if len(set(prts1) - set(prts2)) == 0:
                        if sum(np.abs(np.array(cpt1) - np.array(cpt2))) < 0.0001:
                            return True     
                return False
            
            self.bridges = []
            for bridge in bridges:
                uparents = dict()
                for parent in self.indirect_parents[bridge]:
                    found = False
                    if len(uparents) == 0:
                        found = True
                        uparents[parent] = [parent]
                    else:
                        for up in uparents.keys():
                            if identical(up, parent):
                                uparents[up].append(parent)
                                found = True
                                break
                    if not found:
                        uparents[parent] = [parent]
                                
                if len(uparents)==1:
                    ##transform the bridge into a normal node
                    arbparent = uparents.keys()[0]
                    bridge.name = arbparent.name
                    bridge.s = arbparent.s
                    bridge.o = arbparent.o
                    bridge.t = arbparent.t
                    bridge.l = arbparent.l
                    bridge.extendable = arbparent.extendable
                    bridge.shared = arbparent.shared
                    self.ndict[bridge] = self.ndict[arbparent]
                    for parent in uparents[arbparent]:
                        del self.ndict[parent]
                    for parent in self.private_parents[bridge]:
                        del self.ndict[parent]                    
                else:                        
                    self.bridges.append(bridge)
                
            obsnew = []
            for obs in self.obs:
                node = obs[0]
                if self.name(node) in name2bridge.keys():     
                    bridge = name2bridge[self.name(node)]
                    obsadded = False
                    for obs2 in obsnew:
                        if bridge == obs2[0]:
                            obsadded = True
                            break                            
                    if not obsadded:
                        obsnew.append([bridge, obs[1], obs[2]])
                else:
                    obsnew.append(obs)
            self.obs = obsnew
            
                
        buildbridges()
        daifactors = dai.VecFactor()
        nlist = toposort(self.ndict)
        self.node2label = dict()
        self.node2daiVar = dict()

        for graphid,node in enumerate(nlist):
            self.node2label[node] = graphid
            self.node2daiVar[node] = dai.Var(graphid,2)
        
        for node,context in self.ndict.iteritems():
#             print "";
            vlist = []
            parents = context[0]
            ptable = context[1]
            if len(parents)==0:
               vset = self.node2daiVar[node]
#                print "Node:", node.name
            else:
                for parent in parents:
                    vlist.append(self.node2daiVar[parent])
#                     print "Parent:", parent.name;
                vlist.append(self.node2daiVar[node])
                vset = dai.VarSet(vlist[0],vlist[1])
                for node in vlist[2:]:
                    vset.append(node)
            f = dai.Factor(vset)
            daifactors.append(p2cpt(f, parents, ptable, self.node2label))        
        self.dainetwork = dai.FactorGraph(daifactors)
        self.dainetwork.WriteToFile("tempdai.fg")
#        self.setupped = True

    def computeFactors(self):
        # Calculate joint probability of all four variables
        prob_Factor = dai.Factor();
        for i in range(self.dainetwork.nrFactors()):
            prob_Factor *= self.dainetwork.factor(i);
        prob_Factor.normalize(); # Not necessary: a Bayesian network is already normalized by definition
        
        print "";
        for node in self.ndict:
            daiVar = self.node2daiVar[node];
            nProb = prob_Factor.marginal(dai.VarSet(daiVar))[1];
            print self.node2label[node], "- " + node.name+":", nProb;
        print "";
        

        ##run BP clamped and unclamped 
        ##will need to clamp the mex nodes
    def run(self):
        if not self.ran:
            self.bp_unclamped = dwrap.BPwrapper()
            for n in self.mexnodes:
                self.bp_unclamped.clamp(self.node2label[n], True)
            self.bp_unclamped.run()

            self.bp_clamped = dwrap.BPwrapper()
            for n in self.mexnodes:
                self.bp_clamped.clamp(self.node2label[n], True)
            for obs in self.obs:
                nid = self.node2label[obs[0]]
                self.bp_clamped.clamp(nid, obs[1])
            self.bp_clamped.run()
#        self.ran = True

    def postprob(self, nodes):
        self.run()
        p = []
        for node in nodes:
            self.bp_clamped.addVar(self.node2label[node])
            p.append(self.bp_clamped.getJoint(0))
        self.bp_clamped.resetVars()
        self.bp_clamped.dealloc();
        self.bp_unclamped.dealloc();
        return p

    def priorprob(self, nodes):
#         self.computeFactors();
        self.run()
        p = []
        for node in nodes:
            self.bp_unclamped.addVar(self.node2label[node])
            p.append(self.bp_unclamped.getJoint(0))
        self.bp_unclamped.resetVars()
        self.bp_clamped.dealloc();
        self.bp_unclamped.dealloc();        
        return p

##    def postprob(self, nodes):
##        bp_clamped = dwrap.BPwrapper()
##        for n in self.mexnodes:
##            bp_clamped.clamp(self.node2label[n], True)
##        for obs in self.obs:
##            nid = self.node2label[obs[0]]
##            bp_clamped.clamp(nid, obs[1])
##        bp_clamped.run()
##        p = []
##        for node in nodes:
##            bp_clamped.addVar(self.node2label[node])
##            p.append(bp_clamped.getJoint(0))
##        bp_clamped.resetVars()
##        return p
##
##    def priorprob(self, nodes):
##        bp_unclamped = dwrap.BPwrapper()
##        for n in self.mexnodes:
##            bp_unclamped.clamp(self.node2label[n], True)
##        bp_unclamped.run()
##        p = []
##        for node in nodes:
##            bp_unclamped.addVar(self.node2label[node])
##            p.append(bp_unclamped.getJoint(0))
##        bp_unclamped.resetVars()
##        return p
    

    
    def prt(self, name = "", fullname = False, obs=False, bridges = False):
        if bridges:
            keys = self.bridges
        elif obs:
            keys = map(lambda o: o[0], self.obs)
        elif fullname:
            keys = []
            for key in self.ndict.keys():
                if self.name(key) == fullname:
                    keys.append(key)
        elif name == "":
            keys = self.ndict.keys()
        else:
            keys = self.which(name = name)
        for n in keys:
            parentheses = ""
            for obs in self.obs:                
                if n == obs[0]:
                    parentheses = " (Obs value: "+str(obs[1])+")"
                    break
            line = ""
            for parent in self.ndict[n][0]:
                line = line+" -"+self.name(parent)
                if parent.weight == "f":
                    line = line + " (F)"
            print self.name(n)+parentheses+": "+ line
            line = ""
            getcontext().prec = 3
            for i in self.ndict[n][1]:
                line += " "+str(Decimal(str(i)).quantize(Decimal('.01'), rounding = ROUND_DOWN))            
            print "prob:"+line
    
    # def bp(self, clamp=True):
    #     if not self.clamped and clamp:
    #         self.BP = dwrap.BPwrapper()
    #         for obs in self.obs:
    #             nid = self.node2label[obs[0]]
    #             BP.clamp(nid, obs[1])

    #     if withobs

    def prob(self, node):
        return self.postprob([node])[0]
    # def prob(self,node): 
    #     BP = dwrap.BPwrapper()
    #     for obs in self.obs:
    #         nid = self.node2label[obs[0]]
    #         BP.clamp(nid, obs[1])
    #     BP.run()
    #     BP.addVar(self.node2label[node])
    #     rval = BP.getJoint(0)
    #     BP.dealloc()
    #     return rval
    
    def write2dot(self,fname = "graph.dot", plabels=False):
        node2label = self.node2label
        f = open(fname, "w")
        f.write("digraph G {\n")
        #f.write("node[shape=circle, width=0.4,fixedsize=true];\n")
        f.write("node[shape=circle, width=0.4];\n")
        for node in self.ndict.keys():
            if plabels:
                l = ""
                for i in self.ndict[node][1]:                
                    l += "x"+str(Decimal(str(10*i)).quantize(Decimal('1'), rounding = ROUND_DOWN))
            else:
                l = "\""+ node.name +"\""
            f.write(str(node2label[node]))
            if node in map(lambda x : x[0], self.obs):
                f.write("[label="+l+",style=filled,color=blue]")
            elif node.name == NN.WAITING:
                f.write("[label="+l+",style=filled,color=green]")
            elif node.name == NN.STAY_PUT:
                f.write("[label="+l+",style=filled,color=yellow]")
            elif node.name == NN.HOLDING_OBJ:
                f.write("[label="+l+",style=filled,color=black]")
            elif node.name == NN.EXCHANGE_DROP:
                f.write("[label="+l+",style=filled,color=red]")
            else:
                #f.write("[label="+self.name(node)+"]")
                f.write("[label="+l+"]")

            f.write(";\n")
            for parent in self.ndict[node][0]:
                f.write(str(node2label[parent])+" -> "+ str(node2label[node])+";\n")
                #f.write(self.name(parent)+" -> "+ self.name(node)+";\n")
        
        f.write("}")
        f.close()
        

    
                        
    def prior(self):
        raise Exception('Unimplemented function')
#        s = 0
#        for obs in self.obs:
#            if value:
#                s += log(obs[2])
#            else:
#                s += log(1-obs[2])
#        return(s)

    def tobin(self,entry, n):
        digits = []
        ndigits = 0
        while (entry > 0):
            digits = [entry - 2*int(float(entry)/float(2))]+digits
            ndigits = ndigits+1
            entry = entry/2        
        while (ndigits < n):
            digits = [0]+digits
            ndigits = ndigits+1
        return(digits)

    def frombin(self,digits):
        entry = 0
        digits.reverse()
        for i,d in enumerate(digits):
            entry = pow(2,i)*d +entry
        return(entry)

    def name(self, nodes):
        def nname(node):
            if node.extendable:
                sstr = ""
                for s in node.s:
                    sstr = sstr+('%03d'%s)
                ostr = ""
                for o in node.o:
                    ostr = ostr+('%03d'%o)
                tstr = ""
                for t in node.t:
                    tstr = tstr+('%03d'%t)
                return(node.name+"_"+sstr+"_"+ostr+"_"+tstr)
            else:
                return(node.name)
            
        if type(nodes).__name__=='list':
            return(map(nname, nodes))
        return(nname(nodes))
